#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <mosquitto.h>
#include <jsoncpp/json/json.h>
#include <string>
#include <cstring>
#include "json_data.hpp"
class SeatMqttBridge : public rclcpp::Node
{
public:
    SeatMqttBridge() : Node("seat_mqtt_bridge")
    {
        declare_parameter_yaml();      
        get_parameters_yaml();
        // 初始化MQTT
        mosquitto_lib_init();
        mqtt_client_ = mosquitto_new(nullptr, true, this);
        
        if (!mqtt_client_) {
            RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
            return;
        }

        // 设置MQTT回调
        mosquitto_message_callback_set(mqtt_client_, on_mqtt_message);
        mosquitto_connect_callback_set(mqtt_client_, on_connect);
        mosquitto_disconnect_callback_set(mqtt_client_, on_disconnect);

         // 启动连接线程
        connect_thread_ = std::thread(&SeatMqttBridge::connect_loop, this);

           
        // 启动MQTT循环线程
        int rc = mosquitto_loop_start(mqtt_client_);
        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Could not start MQTT loop: %s", mosquitto_strerror(rc));
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
            return;
        }
        
        // 创建ROS订阅者（接收ROS消息并转发到MQTT）
        //  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_to_mqtt0_rosname_subscriber_;
        // rclcpp::Publisher<std_msgs::msg::String>::SharedPtr mqtt_to_ros1_rosname_publisher_;
        ros_to_mqtt0_rosname_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            ros_to_mqtt0_rosname,
            10,
            std::bind(&SeatMqttBridge::ros_message_callback_0, this, std::placeholders::_1)
        );
        ros_to_mqtt2_rosname_subscriber_ = this->create_subscription<std_msgs::msg::String>(
            ros_to_mqtt2_rosname,
            10,
            std::bind(&SeatMqttBridge::ros_message_callback_2, this, std::placeholders::_1)
        );
        RCLCPP_INFO(this->get_logger(),"ros_to_mqtt0_rosname =%s",ros_to_mqtt0_rosname.c_str());
        RCLCPP_INFO(this->get_logger(),"ros_to_mqtt2_rosname =%s",ros_to_mqtt2_rosname.c_str());
        // 创建ROS发布者（将MQTT消息转发到ROS）
        mqtt_to_ros1_rosname_publisher_ = this->create_publisher<std_msgs::msg::String>(
            mqtt_to_ros1_rosname,
            10
        );
        // if(ros2_send==1)
        //   timer_ = this->create_wall_timer(std::chrono::seconds(2), std::bind(&SeatMqttBridge::time_publish_message, this));
        // RCLCPP_INFO(this->get_logger(), "Seat MQTT bridge initialized. ROS pubtopic: %s, MQTT pubtopic: %s",
        //           ros_pubtopic.c_str(), mqtt_pubtopic_.c_str());
        // RCLCPP_INFO(this->get_logger(), "Seat MQTT bridge initialized. ROS subtopic: %s, MQTT subtopic: %s",
        //           ros_subtopic.c_str(), mqtt_subtopic_.c_str());
    }

    ~SeatMqttBridge()
    {
             // 停止连接线程
        stop_connect_thread_ = true;
        if (connect_thread_.joinable()) {
            connect_thread_.join();
        }
        
        if (mqtt_client_) {
            mosquitto_loop_stop(mqtt_client_, true);
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
        }
        mosquitto_lib_cleanup();
        // if (mqtt_client_) {
        //     mosquitto_loop_stop(mqtt_client_, true);
        //     mosquitto_disconnect(mqtt_client_);
        //     mosquitto_destroy(mqtt_client_);
        // }
        // mosquitto_lib_cleanup();
    }

    // 处理从MQTT接收的消息并发布到ROS
    void handle_mqtt_message(const std::string& topic,const std::string& payload)
    {
        
        // 发布到ROS
        auto msg = std_msgs::msg::String();
       if(topic==mqtt_to_ros1_mqttname)
        {
       // RCLCPP_INFO(this->get_logger(),"Receive  mqtttopic: %s",topic.c_str());
       // RCLCPP_INFO(this->get_logger(), "Published MQTT data to ROS: %s", payload.c_str());
         msg.data = payload;  // 直接转发JSON字符串
        mqtt_to_ros1_rosname_publisher_->publish(msg);
        
        }
    //     else RCLCPP_INFO(this->get_logger(),"Receive  mqtttopic: %s",topic.c_str());
    //     //std::string nn="mqtt_zhua_info";
    //    if(topic==mqtt_zhua_info_)
    //    {
    //        msg.data = payload;  // 直接转发JSON字符串
    //      ros_publisher_->publish(msg);
    //       RCLCPP_INFO(this->get_logger(),"Receive  mqtttopic: %s",topic.c_str());
    //    }
    }

private:
    // ROS回调：接收ROS消息并转发到MQTT
    void ros_message_callback_0(const std_msgs::msg::String::SharedPtr msg)
    {
     

          if (!is_connected_) {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT broker, cannot publish message");
            return;
        }
        // 发布到MQTT
        int rc = mosquitto_publish(
            mqtt_client_,
            nullptr,
            ros_to_mqtt0_mqttname.c_str(),
            msg->data.length(),
            msg->data.c_str(),
            0,
            false
        );

        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish to MQTT: %s", mosquitto_strerror(rc));
        } else {
            RCLCPP_INFO(this->get_logger(), "Published ROS data to MQTT: %s", msg->data.c_str());
        }
    }
    void ros_message_callback_2(const std_msgs::msg::String::SharedPtr msg)
    {
     

          if (!is_connected_) {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT broker, cannot publish message");
            return;
        }
        // 发布到MQTT
        int rc = mosquitto_publish(
            mqtt_client_,
            nullptr,
            ros_to_mqtt2_mqttname.c_str(),
            msg->data.length(),
            msg->data.c_str(),
            0,
            false
        );

        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish to MQTT: %s", mosquitto_strerror(rc));
        } else {
            RCLCPP_INFO(this->get_logger(), "Published ROS data to MQTT: %s", msg->data.c_str());
        }
    }
   // 连接成功回调
    static void on_connect(struct mosquitto* mosq, void* obj, int rc)
    {
        if (!obj) return;
        
        SeatMqttBridge* bridge = static_cast<SeatMqttBridge*>(obj);
        
        if (rc == 0) {
            bridge->is_connected_ = true;
            RCLCPP_INFO(bridge->get_logger(), "Successfully connected to MQTT broker");
            // 连接成功后订阅主题
            bridge->resubscribe_topics();
        } else {
            bridge->is_connected_ = false;
            RCLCPP_ERROR(bridge->get_logger(), "Failed to connect to MQTT broker: %s", mosquitto_strerror(rc));
        }
    }
      
    // 断开连接回调
    static void on_disconnect(struct mosquitto* mosq, void* obj, int rc)
    {
        if (!obj) return;
        
        SeatMqttBridge* bridge = static_cast<SeatMqttBridge*>(obj);
        
        if (rc != 0) { // 非自愿断开连接
            bridge->is_connected_ = false;
            RCLCPP_WARN(bridge->get_logger(), "Disconnected from MQTT broker. Will attempt to reconnect...");
        }
    }

    // MQTT消息回调函数（静态方法）
    static void on_mqtt_message(struct mosquitto* mosq, void* obj, const struct mosquitto_message* msg)
    {
        if (!obj || !msg->payload) return;

        SeatMqttBridge* bridge = static_cast<SeatMqttBridge*>(obj);
        std::string payload(static_cast<char*>(msg->payload), msg->payloadlen);
        std::string topic(msg->topic);  // 通过 msg->topic 获取主题
        bridge->handle_mqtt_message(topic,payload);
    }
    // 连接循环，处理重连逻辑
    void connect_loop()
    {
        int retry_count = 0;
        const int max_retry_delay = 5; // 最大重连延迟（秒）
        
        while (rclcpp::ok() && !stop_connect_thread_) {
            if (!is_connected_) {
                RCLCPP_INFO(this->get_logger(), "Trying to connect to MQTT broker: %s:%d", 
                           mqtt_host_.c_str(), mqtt_port_);
                
                int rc = mosquitto_connect(mqtt_client_, mqtt_host_.c_str(), mqtt_port_, 60);
                if (rc != 0) {
                    int delay = std::min(1 << retry_count, max_retry_delay); // 指数退避
                    RCLCPP_ERROR(this->get_logger(), "Could not connect to MQTT broker: %s. Retrying in %d seconds...",
                                mosquitto_strerror(rc), delay);
                    
                    // 等待重试
                    std::this_thread::sleep_for(std::chrono::seconds(delay));
                    retry_count++;
                } else {
                    // 连接请求已发送，等待回调确认
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                }
            } else {
                // 已连接，短暂休眠
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
    }
        // 重新订阅主题
    void resubscribe_topics()
    {
        int rc = mosquitto_subscribe(mqtt_client_, nullptr, mqtt_to_ros1_mqttname.c_str(), 0);
        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Could not subscribe to MQTT topic: %s", mosquitto_strerror(rc));
        } else {
            RCLCPP_INFO(this->get_logger(), "Re-subscribed to MQTT topic: %s", mqtt_to_ros1_mqttname.c_str());
        }
    }
    void declare_parameter_yaml(void)
    {
         // 声明并获取参数
        declare_parameter("ros2_send",0);
        declare_parameter("mqtt.host", "localhost");
        declare_parameter("mqtt.port", 1883);
        declare_parameter("mqtt_sub.mqtt_to_ros1_mqttname", "noname");
        declare_parameter("mqtt_sub.mqtt_to_ros1_rosname", "noname");
        declare_parameter("ros_sub.ros_to_mqtt0_rosname", "noname");
        declare_parameter("ros_sub.ros_to_mqtt0_mqttname", "noname");
        declare_parameter("ros_sub.ros_to_mqtt2_rosname", "noname");
        declare_parameter("ros_sub.ros_to_mqtt2_mqttname", "noname");
      //  this->declare_parameter("mqtt.mqtt_zhua_info", "mqtt_zhua_info_");
    }
    void get_parameters_yaml(void)
    {
          ros2_send =get_parameter("ros2_send").as_int();
        mqtt_host_ = get_parameter("mqtt.host").as_string();
        mqtt_port_ = get_parameter("mqtt.port").as_int();
        // std::string ros_to_mqtt0_rosname,ros_to_mqtt0_mqttname;
        // std::string mqtt_to_ros1_rosname,mqtt_to_ros1_mqttname;
        ros_to_mqtt0_rosname = get_parameter("ros_sub.ros_to_mqtt0_rosname").as_string();
        ros_to_mqtt0_mqttname = get_parameter("ros_sub.ros_to_mqtt0_mqttname").as_string();
        ros_to_mqtt2_rosname = get_parameter("ros_sub.ros_to_mqtt2_rosname").as_string();
        ros_to_mqtt2_mqttname = get_parameter("ros_sub.ros_to_mqtt2_mqttname").as_string();

        mqtt_to_ros1_rosname = get_parameter("mqtt_sub.mqtt_to_ros1_rosname").as_string();
        mqtt_to_ros1_mqttname = get_parameter("mqtt_sub.mqtt_to_ros1_mqttname").as_string();
       // mqtt_zhua_info_ = get_parameter("mqtt.mqtt_zhua_info").as_string();
       
    }
    void time_publish_message(void)  ///测试用
    {
      count++;
      msg_send.seatx=1.1+count;
      msg_send.seaty=1.2;
      msg_send.seatz=1.3;
      msg_send.name = "hello";
      msg_send.set_json_msg();
        // 发布到MQTT
        // int rc = mosquitto_publish(
        //     mqtt_client_,
        //     nullptr,
        //     mqtt_pubtopic_.c_str(),
        //     msg_send.json_msg.length(),
        //     msg_send.json_msg.c_str(),
        //     0,
        //     false
        // );
    }
    struct mosquitto* mqtt_client_;
    std::string ros_to_mqtt0_rosname,ros_to_mqtt0_mqttname;
    std::string mqtt_to_ros1_rosname,mqtt_to_ros1_mqttname;
    std::string ros_to_mqtt2_rosname,ros_to_mqtt2_mqttname;
    std::string mqtt_zhua_info_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_to_mqtt0_rosname_subscriber_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_to_mqtt2_rosname_subscriber_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr mqtt_to_ros1_rosname_publisher_;
    JsonData_convert msg_rec;
    JsonData_convert msg_send;
    int ros2_send;
    int count=0;
    bool is_connected_;
    std::thread connect_thread_;
    bool stop_connect_thread_ = false;
    std::string mqtt_host_;
    int mqtt_port_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SeatMqttBridge>());
    rclcpp::shutdown();
    return 0;
}
    
